#include "commWithBridge.h"
#include <QDebug>

// static RobotStatus robotStatus;

commWithBridge::commWithBridge(MainWindow * mainWindow, QObject *parent) 
    : QObject(parent)
    , mainWindow(mainWindow)
{
    commRosToGuiSig = new CommSignal(this);
    commWithBoard_ = new commWithBoard();

    tcpSocket = new QTcpSocket();
    udpSocket = new QUdpSocket();
    tcpConnTimer = new QTimer();
    tcpRecvTimer = new QTimer();

    curr_ctrl_mode = 2; // ctrl_mode: 1-手动 2-自动
    curr_running_mode = 3; // running_mode: 1:启动 2:暂停 3:停止
    curr_step = 0;

    connect(tcpSocket, &QTcpSocket::connected, this, &commWithBridge::handleConnected);
    connect(tcpSocket, &QTcpSocket::disconnected, this, &commWithBridge::handleDisconnected);
    connect(tcpSocket, &QTcpSocket::readyRead, this, &commWithBridge::handleTcpReadyRead);
    connect(udpSocket, &QUdpSocket::readyRead, this, &commWithBridge::handleUdpReadyRead);
    // 连接错误信号到错误处理slot
    connect(tcpSocket, &QAbstractSocket::errorOccurred, this, &commWithBridge::handleErr);
    connect(tcpConnTimer, &QTimer::timeout, this, &commWithBridge::handleTcpConnection);
    connect(tcpRecvTimer, &QTimer::timeout, this, &commWithBridge::handleTcpRecvTimeout);

    connect(commWithBoard_->boardSigToBridge, &CommSignal::commSignal, this, &commWithBridge::processBoardSig);
}

commWithBridge::~commWithBridge()
{

}

void commWithBridge::commWithBridgeInit()
{
    qDebug()<< "commWithBridgeInit is called.";
    if(mainWindow == nullptr)
    {
        qDebug()<< "In commWithBridgeInit,mainWindow is nullptr.";
        return ;
    }

    if(mainWindow->guiToSysBridgeSig == nullptr)
    {
        qDebug()<< "In commWithBridgeInit,mainWindow->guiToSysBridgeSig is nullptr.";
        return;
    }

    connect(mainWindow->guiToSysBridgeSig, &CommSignal::commSignal, this, &commWithBridge::processGuiSig);

    initJsonMsg();
    
    QJsonDocument doc(heartbeat_msg);
    heartBeatByteArr = doc.toJson(QJsonDocument::Compact);
}

void commWithBridge::processGuiSig(int guiSig)
{
    // qDebug() << "Received sig from GUI.";
    if(guiSig >= GUI_SIG_INI && guiSig <= GUI_SIG_END)
    {
        qDebug() << "Sig is AUTO sig.";
        processAutoSig(guiSig);
    }
    else if(guiSig >= GUI_SIG_MOVE_INI && guiSig <= GUI_SIG_MOVE_END)
    {
        // qDebug() << "Sig is MOVE sig.";
        processMoveSig(guiSig);
    }
    else if(guiSig >= GUI_SIG_GRIPPER_INI && guiSig <= GUI_SIG_GRIPPER_END)
    {
        qDebug() << "Sig is GRIPPER sig.";
        processGripperSig(guiSig);
    }
    else if(guiSig >= GUI_SIG_ARM_L_INI && guiSig <= GUI_SIG_ARM_L_END)
    {
        qDebug() << "Sig is LEFT_ARM_SIG.";
        processLeftArmSig(guiSig);
    }
    else if(guiSig >= GUI_SIG_ARM_R_INI && guiSig <= GUI_SIG_ARM_R_END)
    {
        qDebug() << "Sig is RIGHT_ARM_SIG.";
        processRightArmSig(guiSig);
    }
    else if(guiSig >= GUI_ROS_TEST_SIG_INI && guiSig <= GUI_ROS_TEST_SIG_END)
    {
        qDebug() << "Sig is TEST_SIG.";
        processTestSig(guiSig);
    }
}

void commWithBridge::processBoardSig(int boardSig)
{
    qDebug() << "Received sig from board.";
    switch(boardSig)
    {
        case GUI_SIG_START:
            qDebug() << "Process START sig.";
            sendAutoStartMsg();
        break;
        case GUI_SIG_STOP:
            qDebug() << "Process STOP sig.";
            sendAutoStopMsg();
        break;
        case GUI_SIG_PAUSE:
            qDebug() << "Process PAUSE sig.";
            sendAutoPauseMsg();
        break;
        case GUI_SIG_GRIPPER_OPEN_L:
            qDebug() << "Process GRIPPER_OPEN_L sig.";
            sendLeftGripperOpenMsg();
        break;
        case GUI_SIG_GRIPPER_CLOSE_L:
            qDebug() << "Process GRIPPER_CLOSE_L sig.";
            sendLeftGripperCloseMsg();
        break;
        case GUI_SIG_GRIPPER_OPEN_R:
            qDebug() << "Process GRIPPER_OPEN_R sig.";
            sendRightGripperOpenMsg();
        break;
        case GUI_SIG_GRIPPER_CLOSE_R:
            qDebug() << "Process GRIPPER_CLOSE_R sig.";
            sendRightGripperCloseMsg();
        break;
        case GUI_SIG_MOVE_F:
            qDebug() << "Process MOVE_F sig.";
            sendMoveForwardMsg();
        break;
        case GUI_SIG_MOVE_B:
            qDebug() << "Process MOVE_B sig.";
            sendMoveBackwardMsg();
        break;
        case GUI_SIG_MOVE_L:
            qDebug() << "Process MOVE_L sig.";
            sendMoveLeftMsg();
        break;
        case GUI_SIG_MOVE_R:
            qDebug() << "Process MOVE_R sig.";
            sendMoveRightMsg();
        break;
        default:
        break;
    }
}

void commWithBridge::processAutoSig(int guiSig)
{
    static QHash<int, QDateTime> recentSignals;  // 信号ID -> 处理时间
    static QMutex signalMutex;
    
    QMutexLocker locker(&signalMutex);
    
    // 当前时间
    QDateTime currentTime = QDateTime::currentDateTime();
    
    // 清理超过500ms的信号
    QList<int> expiredSignals;
    for (auto it = recentSignals.begin(); it != recentSignals.end(); ++it) {
        if (it.value().msecsTo(currentTime) > 500) {
            expiredSignals.append(it.key());
        }
    }
    for (int sig : expiredSignals) {
        recentSignals.remove(sig);
    }
    
    // 检查信号是否在时间窗口内已处理
    if (recentSignals.contains(guiSig)) {
        qDebug() << "过滤重复信号:" << guiSig;
        return;
    }
    
    // 记录信号处理时间
    recentSignals[guiSig] = currentTime;
    switch(guiSig)
    {
        case GUI_SIG_START:
            qDebug() << "Process START sig.";
            sendAutoStartMsg();
        break;
        case GUI_SIG_STOP:
            qDebug() << "Process STOP sig.";
            sendAutoStopMsg();
        break;
        case GUI_SIG_PAUSE:
            qDebug() << "Process PAUSE sig.";
            sendAutoPauseMsg();
        break;
        case GUI_SIG_AUTO_TO_MANUAL:
            qDebug() << "Process AUTO_TO_MANUAL sig.";
            SendAutoToManualMsg();
        break;
        case GUI_SIG_MANUAL_TO_AUTO:
            qDebug() << "Process MANUAL_TO_AUTO sig.";
            SendManualToAutoMsg();
        break;
        default:
        break;
    }
}

void commWithBridge::processMoveSig(int guiSig)
{
    switch(guiSig)
    {
        case GUI_SIG_MOVE_F:
            // qDebug() << "Process MOVE_F sig.";
            sendMoveForwardMsg();
        break;
        case GUI_SIG_MOVE_B:
            qDebug() << "Process MOVE_B sig.";
            sendMoveBackwardMsg();
        break;
        case GUI_SIG_MOVE_L:
            qDebug() << "Process MOVE_L sig.";
            sendMoveLeftMsg();
        break;
        case GUI_SIG_MOVE_R:
            qDebug() << "Process MOVE_R sig.";
            sendMoveRightMsg();
        break;
        case GUI_SIG_ROBOT_EN:
            qDebug() << "Process ROBOT_EN sig.";
            sendRobotEnMsg();
        break;
        case GUI_SIG_ROBOT_DIS:
            qDebug() << "Process ROBOT_DIS sig.";
            sendRobotDisMsg();
        break;
        default:
        break;
    }
}

void commWithBridge::processGripperSig(int guiSig)
{
    switch(guiSig)
    {
        case GUI_SIG_GRIPPER_OPEN_L:
            qDebug() << "Process LEFT_GRIPPER_OPEN sig.";
            sendLeftGripperOpenMsg();
        break;
        case GUI_SIG_GRIPPER_CLOSE_L:
            qDebug() << "Process LEFT_GRIPPER_CLOSE sig.";
            sendLeftGripperCloseMsg();
        break;
        case GUI_SIG_GRIPPER_OPEN_R:
            qDebug() << "Process RIGHT_GRIPPER_OPEN sig.";
            sendRightGripperOpenMsg();
        break;
        case GUI_SIG_GRIPPER_CLOSE_R:
            qDebug() << "Process RIGHT_GRIPPER_CLOSE sig.";
            sendRightGripperCloseMsg();
        break;
        default:
        break;
    }
}

void commWithBridge::processLeftArmSig(int guiSig)
{
    switch(guiSig)
    {
        case GUI_SIG_ARM_XI_L:
            qDebug() << "Process LEFT_ARM_X_INC sig.";
            sendLeftArmXIMsg();
        break;
        case GUI_SIG_ARM_XD_L:
            qDebug() << "Process LEFT_ARM_X_DEC sig.";
            sendLeftArmXDMsg();
        break;
        case GUI_SIG_ARM_YI_L:
            qDebug() << "Process LEFT_ARM_Y_INC sig.";
            sendLeftArmYIMsg();
        break;
        case GUI_SIG_ARM_YD_L:
            qDebug() << "Process LEFT_ARM_Y_DEC sig.";
            sendLeftArmYDMsg();
        break;
        case GUI_SIG_ARM_ZI_L:
            qDebug() << "Process LEFT_ARM_Z_INC sig.";
            sendLeftArmZIMsg();
        break;
        case GUI_SIG_ARM_ZD_L:
            qDebug() << "Process LEFT_ARM_Z_DEC sig.";
            sendLeftArmZDMsg();
        break;
        case GUI_SIG_ARM_RXI_L:
            qDebug() << "Process LEFT_ARM_RXI_INC sig.";
            sendLeftArmRXIMsg();
        break;
        case GUI_SIG_ARM_RXD_L:
            qDebug() << "Process LEFT_ARM_RXD_INC sig.";
            sendLeftArmRXDMsg();
        break;
        case GUI_SIG_ARM_RYI_L:
            qDebug() << "Process LEFT_ARM_RYI_INC sig.";
            sendLeftArmRYIMsg();
        break;
        case GUI_SIG_ARM_RYD_L:
            qDebug() << "Process LEFT_ARM_RYD_INC sig.";
            sendLeftArmRYDMsg();
        break;
        case GUI_SIG_ARM_RZI_L:
            qDebug() << "Process LEFT_ARM_RZI_INC sig.";
            sendLeftArmRZIMsg();
        break;
        case GUI_SIG_ARM_RZD_L:
            qDebug() << "Process LEFT_ARM_RZD_INC sig.";
            sendLeftArmRZDMsg();
        break;
        default:
        break;
    }
}

void commWithBridge::processRightArmSig(int guiSig)
{
    switch(guiSig)
    {
        case GUI_SIG_ARM_XI_R:
            qDebug() << "Process RIGHT_ARM_X_INC sig.";
            sendRightArmXIMsg();
        break;
        case GUI_SIG_ARM_XD_R:
            qDebug() << "Process RIGHT_ARM_X_DEC sig.";
            sendRightArmXDMsg();
        break;
        case GUI_SIG_ARM_YI_R:
            qDebug() << "Process RIGHT_ARM_Y_INC sig.";
            sendRightArmYIMsg();
        break;
        case GUI_SIG_ARM_YD_R:
            qDebug() << "Process RIGHT_ARM_Y_DEC sig.";
            sendRightArmYDMsg();
        break;
        case GUI_SIG_ARM_ZI_R:
            qDebug() << "Process RIGHT_ARM_Z_INC sig.";
            sendRightArmZIMsg();
        break;
        case GUI_SIG_ARM_ZD_R:
            qDebug() << "Process RIGHT_ARM_Z_DEC sig.";
            sendRightArmZDMsg();
        break;
        case GUI_SIG_ARM_RXI_R:
            qDebug() << "Process RIGHT_ARM_RXI_INC sig.";
            sendRightArmRXIMsg();
        break;
        case GUI_SIG_ARM_RXD_R:
            qDebug() << "Process RIGHT_ARM_RXD_INC sig.";
            sendRightArmRXDMsg();
        break;
        case GUI_SIG_ARM_RYI_R:
            qDebug() << "Process RIGHT_ARM_RYI_INC sig.";
            sendRightArmRYIMsg();
        break;
        case GUI_SIG_ARM_RYD_R:
            qDebug() << "Process Right_ARM_RYD_INC sig.";
            sendRightArmRYDMsg();
        break;
        case GUI_SIG_ARM_RZI_R:
            qDebug() << "Process RIGHT_ARM_RZI_INC sig.";
            sendRightArmRZIMsg();
        break;
        case GUI_SIG_ARM_RZD_R:
            qDebug() << "Process RIGHT_ARM_RZD_INC sig.";
            sendRightArmRZDMsg();
        break;
        default:
        break;
    }
}

void commWithBridge::processTestSig(int guiSig)
{
    switch(guiSig)
    {
        case GUI_ROS_TEST_SIG_STEP1:
            qDebug() << "Process TEST_SIG_STEP1.";
            commRosToGuiSig->send(ROS_SIG_STEP1_COMPLETED);
        break;
        case GUI_ROS_TEST_SIG_STEP2:
            qDebug() << "Process TEST_SIG_STEP2.";
            commRosToGuiSig->send(ROS_SIG_STEP2_COMPLETED);
        break;
        case GUI_ROS_TEST_SIG_STEP3:
            qDebug() << "Process TEST_SIG_STEP3.";
            commRosToGuiSig->send(ROS_SIG_STEP3_COMPLETED);
        break;
        case GUI_ROS_TEST_SIG_STEP4:
            qDebug() << "Process TEST_SIG_STEP4.";
            commRosToGuiSig->send(ROS_SIG_STEP4_COMPLETED);
        break;
        case GUI_ROS_TEST_SIG_STEP5:
            qDebug() << "Process TEST_SIG_STEP5.";
            commRosToGuiSig->send(ROS_SIG_STEP5_COMPLETED);
        break;
        case GUI_ROS_TEST_SIG_STEP6:
            qDebug() << "Process TEST_SIG_STEP6.";
            commRosToGuiSig->send(ROS_SIG_STEP6_COMPLETED);
        break;
        case GUI_ROS_TEST_SIG_MANUAL_TO_AUTO:
            qDebug() << "Process TEST_SIG_MANUAL_TO_AUTO.";
            commRosToGuiSig->send(ROS_SIG_SWITCHED_AUTO);
        break;
        case GUI_ROS_TEST_SIG_AUTO_TO_MANUAL:
            qDebug() << "Process TEST_SIG_AUTO_TO_MANUAL.";
            commRosToGuiSig->send(ROS_SIG_SWITCHED_MANUAL);
        break;
        case GUI_ROS_TEST_SIG_AUTO_START:
            qDebug() << "Process TEST_SIG_AUTO_START.";
            commRosToGuiSig->send(ROS_SIG_AUTO_START);
        break;
        case GUI_ROS_TEST_SIG_AUTO_STOP:
            qDebug() << "Process TEST_SIG_AUTO_STOP";
            commRosToGuiSig->send(ROS_SIG_AUTO_STOP);
        break;
        case GUI_ROS_TEST_SIG_AUTO_PAUSE:
            qDebug() << "Process TEST_SIG_AUTO_PAUSE";
            commRosToGuiSig->send(ROS_SIG_AUTO_PAUSE);
        break;
        case GUI_ROS_TEST_SIG_STATUS_UPDATE:
            qDebug() << "Process TEST_SIG_STATUS_UPDATE";
            commRosToGuiSig->send(ROS_SIG_STATUS_UPDATE);
        break;
        default:
        break;
    }
}

bool commWithBridge::connToBridge(const QString &host, quint16 tcpPort, quint16 udpPort)
{
	qDebug() << "TCP conn to bridge";
	if(tcpSocket->state() == QAbstractSocket::ConnectedState)
	{
		qDebug() << "TCP already connected to bridge";
	}

	tcpSocket->connectToHost(host, tcpPort);

	if(tcpSocket->waitForConnected(3000))
	{
		qDebug() << "TCP connect to bridge,port" << ":" << tcpPort;
		emit connStatusChanged(true);
        // qDebug() << "TCP send heartbeat to bridge...";
        sendMessage(heartBeatByteArr);
        tcpRecvTimer->start(50);
        // 连接成功后设置断开重连机制
        // 断开已有连接，避免重复连接
        disconnect(tcpSocket, &QTcpSocket::disconnected, this, &commWithBridge::handleDisconnected);
        // 连接断开信号到重连处理函数
        connect(tcpSocket, &QTcpSocket::disconnected, this, &commWithBridge::handleDisconnected);
        tcpConnTimer->start(5000);
    }
	else
	{
		qDebug() << "TCP connect to bridge failed:" << tcpSocket->errorString();
		emit errOccurred(tcpSocket->errorString());

        // 首次连接失败，启动重连定时器
        if(!tcpConnTimer->isActive())
        {
            tcpConnTimer->start(10000);
        }
		return false;
	}

    if(udpSocket->bind(QHostAddress::AnyIPv4, udpPort, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint))
    {
        qDebug() << "UDP bind to bridge,port" << ":" << udpPort;
    }
    else
    {
        qDebug() << "UDP bind to bridge failed:" << udpPort;
        return false;
    }
 
    return true;
}

bool commWithBridge::disconnFromBridge()
{
    qDebug() << "TCP disconnect from bridge...";
    if(tcpConnTimer->isActive())
    {
        tcpConnTimer->stop();
    }

	if(tcpSocket->state() == QAbstractSocket::ConnectedState)
	{
		tcpSocket->disconnectFromHost();
		tcpSocket->waitForDisconnected(1000);
		emit connStatusChanged(false);
		return true;
	}

	return false;
}

void commWithBridge::handleTcpConnection()
{
    // 如果已经连接，切换重连定时至心跳定时
    if(tcpSocket->state() == QAbstractSocket::ConnectedState)
    {
        // qDebug() << "TCP send heartbeat to bridge...";
        sendMessage(heartBeatByteArr);
        tcpRecvTimer->start(50);
        tcpConnTimer->stop();
        tcpConnTimer->start(5000);
        return;
    }
    else
    {
        // 尝试重连
        qDebug() << "TCP reconnection to bridge...";
        tcpSocket->connectToHost("127.0.0.1", 5002);
        if(tcpSocket->waitForConnected(3000))
        {
            qDebug() << "TCP reconnection successfully.";
            emit connStatusChanged(true);

            sendMessage(heartBeatByteArr);
            tcpRecvTimer->start(50);
            // 重连成功后停止定时器
            tcpConnTimer->stop();
            tcpConnTimer->start(5000);
        }
        else 
        {
            qDebug() << "TCP reconnection failed:" << tcpSocket->errorString();
            emit errOccurred("TCP reconnection failed: " + tcpSocket->errorString());
            tcpConnTimer->start(10000);
        }

        udpSocket->bind(QHostAddress::AnyIPv4, 5003, QUdpSocket::ShareAddress | QUdpSocket::ReuseAddressHint);
    }
}

void commWithBridge::handleTcpRecvTimeout()
{
    qDebug() << "TCP recv heartbeat package timeout...";
    disconnFromBridge();
    tcpRecvTimer->stop();
    tcpConnTimer->start(10000);
}

void commWithBridge::sendAutoStartMsg()
{
    QJsonDocument doc(auto_start_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send auto_start_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send auto_start_msg to bridge failed.";
    }
}

void commWithBridge::sendAutoStopMsg()
{
    QJsonDocument doc(auto_stop_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send auto_stop_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send auto_stop_msg to bridge failed.";
    }
}

void commWithBridge::sendAutoPauseMsg()
{
    QJsonDocument doc(auto_pause_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send auto_pause_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send auto_pause_msg to bridge failed.";
    }
}

void commWithBridge::SendAutoToManualMsg()
{
    QJsonDocument doc(auto_manual_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send auto_manual_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send auto_manual_msg to bridge failed.";
    }
}

void commWithBridge::SendManualToAutoMsg()
{
    QJsonDocument doc(manual_auto_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_auto_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_auto_msg8 to bridge failed.";
    }
}

void commWithBridge::sendMoveForwardMsg()
{
    QJsonDocument doc(manual_move_f_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_move_f_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_move_f_msg to bridge failed.";
    }
}

void commWithBridge::sendMoveBackwardMsg()
{
    QJsonDocument doc(manual_move_b_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_move_b_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_move_b_msg to bridge failed.";
    }
}

void commWithBridge::sendMoveLeftMsg()
{
    QJsonDocument doc(manual_move_l_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_move_l_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_move_l_msg to bridge failed.";
    }
}

void commWithBridge::sendMoveRightMsg()
{
    QJsonDocument doc(manual_move_r_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_move_r_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_move_r_msg to bridge failed.";
    }
}

void commWithBridge::sendRobotEnMsg()
{
    QJsonDocument doc(manual_robot_en_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_move_r_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_move_r_msg to bridge failed.";
    }
}

void commWithBridge::sendRobotDisMsg()
{
    QJsonDocument doc(manual_robot_dis_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_move_r_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_move_r_msg to bridge failed.";
    }
}

void commWithBridge::sendLeftGripperOpenMsg()
{
    QJsonDocument doc(manual_gripper_open_l_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_gripper_open_l_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_gripper_open_l_msg to bridge failed.";
    }
}

void commWithBridge::sendLeftGripperCloseMsg()
{
    QJsonDocument doc(manual_gripper_close_l_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_gripper_close_l_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_gripper_close_l_msg to bridge failed.";
    }
}

void commWithBridge::sendRightGripperOpenMsg()
{
    QJsonDocument doc(manual_gripper_open_r_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_gripper_open_r_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_gripper_open_r_msg to bridge failed.";
    }
}

void commWithBridge::sendRightGripperCloseMsg()
{
    QJsonDocument doc(manual_gripper_close_r_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_gripper_close_r_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_gripper_close_r_msg to bridge failed.";
    }
}

void commWithBridge::sendLeftArmXIMsg()
{
    QJsonDocument doc(manual_arm_xi_l_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_xi_l_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_xi_l_msg to bridge failed.";
    }
}

void commWithBridge::sendLeftArmXDMsg()
{
    QJsonDocument doc(manual_arm_xd_l_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_xd_l_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_xd_l_msg to bridge failed.";
    }
}

void commWithBridge::sendLeftArmYIMsg()
{
    QJsonDocument doc(manual_arm_yi_l_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_yi_l_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_yi_l_msg to bridge failed.";
    }
}

void commWithBridge::sendLeftArmYDMsg()
{
    QJsonDocument doc(manual_arm_yd_l_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_yd_l_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_yd_l_msg to bridge failed.";
    }
}

void commWithBridge::sendLeftArmZIMsg()
{
    QJsonDocument doc(manual_arm_zi_l_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_zi_l_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_zi_l_msg to bridge failed.";
    }
}

void commWithBridge::sendLeftArmZDMsg()
{
    QJsonDocument doc(manual_arm_zd_l_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_zd_l_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_zd_l_msg to bridge failed.";
    }
}

void commWithBridge::sendLeftArmRXIMsg()
{
    QJsonDocument doc(manual_arm_rxi_l_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_rxi_l_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_rxi_l_msg to bridge failed.";
    }
}

void commWithBridge::sendLeftArmRXDMsg()
{
    QJsonDocument doc(manual_arm_rxd_l_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_rxd_l_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_rxd_l_msg to bridge failed.";
    }
}

void commWithBridge::sendLeftArmRYIMsg()
{
    QJsonDocument doc(manual_arm_ryi_l_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_ryi_l_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_ryi_l_msg to bridge failed.";
    }
}

void commWithBridge::sendLeftArmRYDMsg()
{
    QJsonDocument doc(manual_arm_ryd_l_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_ryd_l_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_ryd_l_msg to bridge failed.";
    }
}

void commWithBridge::sendLeftArmRZIMsg()
{
    QJsonDocument doc(manual_arm_rzi_l_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_rzi_l_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_rzi_l_msg to bridge failed.";
    }
}

void commWithBridge::sendLeftArmRZDMsg()
{
    QJsonDocument doc(manual_arm_rzd_l_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_rzd_l_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_rzd_l_msg to bridge failed.";
    }
}   

void commWithBridge::sendRightArmXIMsg()
{
    QJsonDocument doc(manual_arm_xi_r_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_xi_r_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_xi_r_msg to bridge failed.";
    }
}

void commWithBridge::sendRightArmXDMsg()
{
    QJsonDocument doc(manual_arm_xd_r_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_xd_r_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_xd_r_msg to bridge failed.";
    }
}

void commWithBridge::sendRightArmYIMsg()
{
    QJsonDocument doc(manual_arm_yi_r_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_yi_r_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_yi_r_msg to bridge failed.";
    }
}

void commWithBridge::sendRightArmYDMsg()
{
    QJsonDocument doc(manual_arm_yd_r_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_yd_r_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_yd_r_msg to bridge failed.";
    }
}

void commWithBridge::sendRightArmZIMsg()
{
    QJsonDocument doc(manual_arm_zi_r_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_zi_r_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_zi_r_msg to bridge failed.";
    }
}

void commWithBridge::sendRightArmZDMsg()
{
    QJsonDocument doc(manual_arm_zd_r_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_zd_r_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_zd_r_msg to bridge failed.";
    }
}

void commWithBridge::sendRightArmRXIMsg()
{
    QJsonDocument doc(manual_arm_rxi_r_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_rxi_r_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_rxi_r_msg to bridge failed.";
    }
}

void commWithBridge::sendRightArmRXDMsg()
{
    QJsonDocument doc(manual_arm_rxd_r_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_rxd_r_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_rxd_r_msg to bridge failed.";
    }
}

void commWithBridge::sendRightArmRYIMsg()
{
    QJsonDocument doc(manual_arm_ryi_r_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_ryi_r_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_ryi_r_msg to bridge failed.";
    }
}

void commWithBridge::sendRightArmRYDMsg()
{
    QJsonDocument doc(manual_arm_ryd_r_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_ryd_r_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_ryd_r_msg to bridge failed.";
    }
}

void commWithBridge::sendRightArmRZIMsg()
{
    QJsonDocument doc(manual_arm_rzi_r_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_rzi_r_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_rzi_r_msg to bridge failed.";
    }
}

void commWithBridge::sendRightArmRZDMsg()
{
    QJsonDocument doc(manual_arm_rzd_r_msg);
    QByteArray sendByteArr = doc.toJson(QJsonDocument::Compact);

    if(sendMessage(sendByteArr))
    {
        qDebug() << "Send manual_arm_rzd_r_msg to bridge OK.";
    }
    else
    {
        qDebug() << "Send manual_arm_rzd_r_msg to bridge failed.";
    }
}

bool commWithBridge::sendMessage(QByteArray &message)
{
    // 使用互斥锁保护发送操作
    static QMutex sendMutex;
    QMutexLocker locker(&sendMutex);

	if(tcpSocket->state() != QAbstractSocket::ConnectedState)
	{
		qDebug() << "TCP not connected to bridge";
		emit errOccurred("TCP not connected to bridge");
		return false;
	}
    // 创建带分隔符的临时消息，避免修改原始消息
    QByteArray messageWithDelimiter = message;
    messageWithDelimiter.append('\n');
    qDebug() << "TCP send msg:" << messageWithDelimiter;

    // 发送带分隔符的消息
    qint64 bytesWritten = tcpSocket->write(messageWithDelimiter);
    
    // 验证是否成功发送了所有字节
    return bytesWritten == messageWithDelimiter.size();
}

bool commWithBridge::isConnected() const
{
	return tcpSocket->state() == QAbstractSocket::ConnectedState;
}

void commWithBridge::handleConnected()
{
	qDebug() << "TCP connected to bridge";
	emit connStatusChanged(true);
}

void commWithBridge::handleDisconnected()
{
	qDebug() << "TCP disconnected from bridge";
	emit connStatusChanged(false);

    if(!tcpConnTimer->isActive())
    {
        tcpConnTimer->start(5000);
    }
}

void commWithBridge::handleTcpReadyRead()
{
	QByteArray jsonMsg = tcpSocket->readAll();
	qDebug() << "TCP received message:" << jsonMsg;
    parseTcpJsonData(jsonMsg);
}

void commWithBridge::handleUdpReadyRead()
{
    // qDebug() << "UDP received message," << "has pending datagram:";
    // qDebug() << "UDP received status message.";
    while(udpSocket->hasPendingDatagrams())
    {
        QByteArray jsonMsg;
        jsonMsg.resize(udpSocket->pendingDatagramSize());

        QHostAddress sender;
        quint16 senderPort;
        
        udpSocket->readDatagram(jsonMsg.data(), jsonMsg.size(), &sender, &senderPort);
        // qDebug() << "jsonMsg.data():" << jsonMsg.data();
        // qDebug() << "jsonMsg.data():" << jsonMsg.data() << ",jsonMsg.size():" << jsonMsg.size() << ",senderPort:" << senderPort;
        // qDebug() << "UDP received message:" << jsonMsg;
        parseUdpJsonData(jsonMsg);
    }
}

void commWithBridge::handleErr(QAbstractSocket::SocketError socketErr)
{
	qDebug() << "Socket error" << socketErr;
	emit errOccurred(tcpSocket->errorString());
}

void commWithBridge::parseTcpJsonData(const QByteArray &jsonData)
{
    QJsonParseError parseErr;
    QJsonDocument doc = QJsonDocument::fromJson(jsonData, &parseErr);

    if(parseErr.error != QJsonParseError::NoError)
    {
        qDebug() << "TCP received Json parsing error:" << parseErr.errorString();
        return;
    }

    QJsonObject root = doc.object();
    if(root.contains("type"))
    {
        if((root["type"].toString() == "heartbeat") && 
            (root["status"].toString() == "alive"))
        {
            tcpRecvTimer->stop();
            // qDebug() << "TCP recv heartbeat pkg,connection OK.";
        }
    }
    return;
}

void commWithBridge::parseUdpJsonData(const QByteArray &jsonData)
{
    QJsonParseError parseErr;
    QJsonDocument doc = QJsonDocument::fromJson(jsonData, &parseErr);

    if(parseErr.error != QJsonParseError::NoError)
    {
        qDebug() << "UDP received Json parsing error:" << parseErr.errorString();
        return;
    }

    QJsonObject rootObj = doc.object();
    if(!rootObj.contains("info"))
    {
        qDebug() << "Missing 'info'";
        return;
    }


    QString infoType = rootObj["info"].toString();
    // qDebug() << "infoType:" << infoType;
    if(infoType != "status" && infoType != "sys_status")
    {
        qDebug() << "Invalid status or sys info.";
        return;
    }

    if(infoType == "status")
    {
        // 解析机械臂在线信息
        if(rootObj.contains("arm_online") && rootObj["arm_online"].isBool())
        {
            robotStatusInfo.arm_info.arm_online = rootObj["arm_onine"].toBool();
        }

        // debug打印使用
        QVector<double> left_pose; 
        QVector<double> left_joints;
        QVector<double> right_pose; 
        QVector<double> right_joints;
        QVector<double> chasis_pose;

        // 解析左机械臂位置信息
        if(rootObj.contains("arm_pose_left") && rootObj["arm_pose_left"].isArray())
        {
            QJsonArray poseArray = rootObj["arm_pose_left"].toArray();
            left_pose.reserve(poseArray.size()); 
            for(int i = 0 ; i < 7; i++)
            {
                robotStatusInfo.arm_info.arm_pose_left[i] = poseArray[i].toDouble();
                left_pose.append(poseArray[i].toDouble());
            }
        }
    
        // 解析左机械臂关节角信息
        if(rootObj.contains("arm_joints_left") && rootObj["arm_joints_left"].isArray())
        {
            QJsonArray jointsArray = rootObj["arm_joints_left"].toArray();
            left_joints.reserve(jointsArray.size());
            for(int i = 0; i < 7; i++)
            {
                robotStatusInfo.arm_info.arm_joints_left[i] = jointsArray[i].toDouble();
                left_joints.append(jointsArray[i].toDouble());
            }
        }

        // 解析右机械臂位置信息
        if(rootObj.contains("arm_pose_right") && rootObj["arm_pose_right"].isArray())
        {
            QJsonArray poseArray = rootObj["arm_pose_right"].toArray();
            right_pose.reserve(poseArray.size()); 
            for(int i = 0 ; i < 7; i++)
            {
                robotStatusInfo.arm_info.arm_pose_right[i] = poseArray[i].toDouble();
                right_pose.append(poseArray[i].toDouble());
            }
        }
    
        // 解析右机械臂关节角信息
        if(rootObj.contains("arm_joints_right") && rootObj["arm_joints_right"].isArray())
        {
            QJsonArray jointsArray = rootObj["arm_joints_right"].toArray();
            right_joints.reserve(jointsArray.size());
            for(int i = 0; i < 7; i++)
            {
                robotStatusInfo.arm_info.arm_joints_right[i] = jointsArray[i].toDouble();
                right_joints.append(jointsArray[i].toDouble());
            }
        }

        // 解析夹爪信息
        if(rootObj.contains("gripper") && rootObj["gripper"].isObject())
        {
            QJsonObject gripperObj = rootObj["gripper"].toObject();
            if(gripperObj.contains("online") && gripperObj["online"].isBool())
            {
                robotStatusInfo.gripper_info.online = gripperObj["online"].toBool();
            }
            if(gripperObj.contains("cur_pose") && gripperObj["cur_pose"].isArray())
            {
                QJsonArray poseArray = gripperObj["cur_pose"].toArray();
                robotStatusInfo.gripper_info.cur_pose_left = poseArray[0].toDouble();
                robotStatusInfo.gripper_info.cur_pose_right = poseArray[1].toDouble();
                
            }
            if(gripperObj.contains("cur_state") && gripperObj["cur_state"].isArray())
            {
                QJsonArray stateArray = gripperObj["cur_state"].toArray();
                robotStatusInfo.gripper_info.cur_state_left = stateArray[0].toBool();
                robotStatusInfo.gripper_info.cur_state_right = stateArray[1].toBool();
            }
        }

        // 解析底盘状态信息
        if(rootObj.contains("chassis") && rootObj["chassis"].isObject())
        {
            QJsonObject chassisObj = rootObj["chassis"].toObject();
            if(chassisObj.contains("online") && chassisObj["online"].isBool())
            {
                robotStatusInfo.chasis_info.online = chassisObj["online"].toBool();     
            }
            if(chassisObj.contains("speed") && chassisObj["speed"].isArray())
            {
                QJsonArray speedArray = chassisObj["speed"].toArray();
                robotStatusInfo.chasis_info.speed[0] = speedArray[0].toDouble();
                robotStatusInfo.chasis_info.speed[1] = speedArray[1].toDouble();
            }
            if(chassisObj.contains("pose") && chassisObj["pose"].isArray())
            {
                QJsonArray poseArray = chassisObj["pose"].toArray();
                chasis_pose.reserve(poseArray.size());
                robotStatusInfo.chasis_info.pose[0] = poseArray[0].toDouble();
                robotStatusInfo.chasis_info.pose[1] = poseArray[1].toDouble();
                robotStatusInfo.chasis_info.pose[2] = poseArray[2].toDouble();
                chasis_pose.append(poseArray[0].toDouble());
                chasis_pose.append(poseArray[1].toDouble());
                chasis_pose.append(poseArray[2].toDouble());
            }
            if(chassisObj.contains("cur_task") && chassisObj["cur_task"].isString())
            {
                robotStatusInfo.chasis_info.cur_task = chassisObj["cur_task"].toString();
            }
        }
        // 视觉系统状态
        if (rootObj.contains("vision") && rootObj["vision"].isObject()) {
            QJsonObject visionObj = rootObj["vision"].toObject();   
            if (visionObj.contains("online") && visionObj["online"].isBool()) {
                robotStatusInfo.vision_online = visionObj["online"].toBool();   
            }
        }

        commRosToGuiSig->send(ROS_SIG_STATUS_UPDATE);

        // qDebug() << "机械臂在线状态：" << robotStatusInfo.arm_info.arm_online;
        // qDebug() << "左机械臂位置：" << left_pose;
        // qDebug() << "左机械臂关节角：" << left_joints;
        // qDebug() << "右机械臂位置：" << right_pose;
        // qDebug() << "右机械臂关节角：" << right_joints;
        // qDebug() << "夹爪在线状态：" << robotStatusInfo.gripper_info.online;
        // qDebug() << "夹爪当前位置，左夹爪：" << robotStatusInfo.gripper_info.cur_pose_left
        //         << "，右夹爪：" << robotStatusInfo.gripper_info.cur_pose_right;
        // qDebug() << "夹爪状态，左夹爪：" << robotStatusInfo.gripper_info.cur_state_left
        //         << "，右夹爪：" << robotStatusInfo.gripper_info.cur_state_right;
        // qDebug() << "底盘在线状态：" << robotStatusInfo.chasis_info.online;
        // qDebug() << "底盘速度：" << robotStatusInfo.chasis_info.speed[0] 
        //         << "," << robotStatusInfo.chasis_info.speed[1];
        // qDebug() << "底盘位置：" << chasis_pose;
        // qDebug() << "底盘当前任务:" << robotStatusInfo.chasis_info.cur_task;
        // qDebug() << "视觉系统在线状态:" << robotStatusInfo.vision_online;
    }
    else if(infoType == "sys_status")
    {
        // 系统状态
        if (rootObj.contains("sys") && rootObj["sys"].isObject()) 
        {
            QJsonObject sysObj = rootObj["sys"].toObject();
            if (sysObj.contains("error") && sysObj["error"].isDouble()) 
            {
                robotStatusInfo.sys_info.error = sysObj["error"].toInt();
            }   
            if (sysObj.contains("step") && sysObj["step"].isDouble()) {
                robotStatusInfo.sys_info.step = sysObj["step"].toInt();
                if(robotStatusInfo.sys_info.step != curr_step)
                {
                    curr_step = robotStatusInfo.sys_info.step;
                    if(robotStatusInfo.sys_info.step == 1)
                    {
                        commRosToGuiSig->send(ROS_SIG_STEP1_COMPLETED);
                    }
                    else if(robotStatusInfo.sys_info.step == 2)
                    {
                        commRosToGuiSig->send(ROS_SIG_STEP2_COMPLETED);
                    }
                    else if(robotStatusInfo.sys_info.step == 3)
                    {
                        commRosToGuiSig->send(ROS_SIG_STEP3_COMPLETED);
                    }
                }
            }
            if (sysObj.contains("msg") && sysObj["msg"].isString()) {
                robotStatusInfo.sys_info.msg = sysObj["msg"].toString(); 
            }
            if (sysObj.contains("running_mode") && sysObj["running_mode"].isDouble()) {
                robotStatusInfo.sys_info.running_mode = sysObj["running_mode"].toInt();
                if(robotStatusInfo.sys_info.running_mode != curr_running_mode)
                {
                    curr_running_mode = robotStatusInfo.sys_info.running_mode;
                    if(robotStatusInfo.sys_info.running_mode == 1)
                    {
                        commRosToGuiSig->send(ROS_SIG_AUTO_START);
                    }
                    else if(robotStatusInfo.sys_info.running_mode == 2)
                    {
                        commRosToGuiSig->send(ROS_SIG_AUTO_PAUSE);
                    }
                    else if(robotStatusInfo.sys_info.running_mode == 3)
                    {
                        commRosToGuiSig->send(ROS_SIG_AUTO_STOP);
                    }
                }
            }
            if (sysObj.contains("ctrl_mode") && sysObj["ctrl_mode"].isDouble()) {
                robotStatusInfo.sys_info.ctrl_mode = sysObj["ctrl_mode"].toInt();
                if(robotStatusInfo.sys_info.ctrl_mode != curr_ctrl_mode)
                {
                    if(robotStatusInfo.sys_info.ctrl_mode != curr_ctrl_mode)
                    {
                        curr_ctrl_mode = robotStatusInfo.sys_info.ctrl_mode;
                        if(robotStatusInfo.sys_info.ctrl_mode == 1)
                        {
                            commRosToGuiSig->send(ROS_SIG_SWITCHED_MANUAL);
                        }
                        else if(robotStatusInfo.sys_info.ctrl_mode == 2)
                        {
                            commRosToGuiSig->send(ROS_SIG_SWITCHED_AUTO);
                        }
                    }
                }
            }
        }
        // qDebug() << "系统错误码:" << robotStatusInfo.sys_info.error;
        // qDebug() << "当前步骤:" << robotStatusInfo.sys_info.step;
        // qDebug() << "系统消息:" << robotStatusInfo.sys_info.msg;
        // qDebug() << "运行模式:" << robotStatusInfo.sys_info.running_mode;
        // qDebug() << "控制模式:" << robotStatusInfo.sys_info.ctrl_mode;
    }
}
